
// Provides complete control of the Protobot over the network (hostname "protobot" port 2000).

#include <Servo.h>
#include <Protobot.h>

Protobot robot;

char IDBytes[2]; // ID bytes of incoming messages
char lengthBytes[2];  // message length bytes of incoming messages
int messageLength;  // message length of incoming message
char messageData[8];  // data bytes of incoming messages
String debugMessage = ""; // holds debug strings we want to send

int moveTime;
byte motorSpeed = 255;
byte sensorThreshold;
unsigned long startTime;


void setup()
{
  robot.begin();  //  setup the i/o
  Serial1.begin(115200);  //  start hardware serial communication
  debugMessage = "robot rebooted!";
  debug();
}


void loop()
{
  while(Serial1.available() > 0)  // wait for command start character
  {
    if(Serial1.read() == '#')
      parseMessage();  // respond to the received message
  }
  
  // other stuff can go here
}


void debug() // sends the human readable info in a debug message
{
  int debugLength = debugMessage.length();
  Serial1.write("#DB"); // debug message ID
  Serial1.write((byte)debugLength/256); // high byte of 2-byte messageLength
  Serial1.write((byte)debugLength%256); // low byte of 2-byte messageLength
  Serial1.print(debugMessage); // send the string
  Serial1.write(0x0A); // bare newline
  debugMessage = ""; // reset the string to empty
}

int joinBytes(byte high, byte low) // turns 2 bytes into an int
{
  return (int)high*256 + (int)low;
}


void parseMessage()
{
  delay(200);  // give the rest of the message a moment to arrive
  // try to read the two ID bytes
  if(Serial1.available() >= 2)
  {
    IDBytes[0] = Serial1.read();
    IDBytes[1] = Serial1.read();
  }
  else
  {
    debugMessage = "no msg id";
    debug();
    return;
  }
  
  // try to read the two message Length bytes
  if(Serial1.available() >= 2)
  {
    lengthBytes[0] = Serial1.read();
    lengthBytes[1] = Serial1.read();
  }
  else
  {
    debugMessage = "no msg length"; // if we timeout
    debug();
    return;
  }
  
  messageLength = joinBytes(lengthBytes[0],lengthBytes[1]);
  // try to read the correct number of data bytes
  if(Serial1.available() >= messageLength)
  {
    for(int i = 0; i < messageLength; i++)
      messageData[i] = Serial1.read();
  }
  else
  {
    debugMessage = "no msg data"; // if we timeout
    debug();
    return;
  }
   
  switch(IDBytes[0]) // message category depends on the first ID byte
  {
    
    case 'L':  // link messages
    {
      if(IDBytes[1] == 'C') // data link check
      {
        debugMessage = "hello";
        debug();
      }
      
      return;
    }
    
    case 'M':  // motor messages
    {
      if(IDBytes[1] == 'F') // move forward
      {
        byte servoPosition = 90;
        robot.turnServo(servoPosition);
        moveTime = 100*(int)messageData[0];
        sensorThreshold = messageData[1];
        startTime = millis();
        boolean scanRight = true;
        robot.forward(motorSpeed);
        while(millis() - startTime < moveTime)  // if no obstacle is found we will overshoot slightly
        {
          if(robot.readSensor() < sensorThreshold)  // if obstacle was seen inside threshold
            break;  // leave the while loop
          if(scanRight)
          {
            servoPosition += 10;
            if(servoPosition >= 120)
              scanRight = false;
          }
          else
          {
            servoPosition -= 10;
            if(servoPosition <= 60)
              scanRight = true;
          }
          robot.turnServo(servoPosition);
        }
        robot.stop();
        byte timeMoved = constrain(((millis() - startTime) / 100),0,255);
        Serial1.write("#MD"); // send the time moved message
        Serial1.write((byte)0); // high byte of 2-byte messageLength
        Serial1.write((byte)1); // low byte of 2-byte messageLength
        Serial1.write(timeMoved);  // return actual time moved
        Serial1.write(0x0A); // bare newline
        return;
      }
      
      if(IDBytes[1] == 'B') // move backward
      {
        moveTime = 100*(int)messageData[0];
        startTime = millis();
        robot.reverse(motorSpeed);
        delay(moveTime);
        robot.stop();
        return;
      }
      
      if(IDBytes[1] == 'L') // spin left
      {
        moveTime = 100*(int)messageData[0];
        startTime = millis();
        robot.turnLeft(motorSpeed);
        delay(moveTime);
        robot.stop();
        return;
      }
      
      if(IDBytes[1] == 'R') // spin right
      {
        moveTime = 100*(int)messageData[0];
        startTime = millis();
        robot.turnRight(motorSpeed);
        delay(moveTime);
        robot.stop();
        return;
      }
      
      return;
    }
    
    case 'S':  // servo messages
    {
      if(IDBytes[1] == 'S')  // servo set
        robot.turnServo(messageData[0]);
        
      return;
    }
    
    case 'R':  // rangefinder messages
    {
      if(IDBytes[1] == 'Q') // rangefinder data request!
      {
        byte distance = robot.readSensor();
        Serial1.write("#RD"); // send the requested data message
        Serial1.write((byte)0); // high byte of 2-byte messageLength
        Serial1.write((byte)1); // low byte of 2-byte messageLength
        Serial1.write(distance);
        Serial1.write(0x0A); // bare newline
        return;
      }
      
      return;
    }
  }
}
